package novemberBot.mapmaking;


import novemberBot.FactoryFloorRobot;
/**uses Dijkstra's algorithm to find the shortest path through the obstacle map
 * @author Antoine Lennartz*/
public class PathFinder {
	/**graph representing the map*/
	private Graph map;
	/**Map object for high sensor*/
	private Map high=FactoryFloorRobot.getInstance().getHighMap();
	
	/**Constructor*/
	public PathFinder(){
		high=FactoryFloorRobot.getInstance().getHighMap();
		map = new Graph(high.getSize());
	}
	

	/**stops the bot rebuilds path and then executes it
	 * @param odoPos target final position
	 * @return start vertice of path*/
	public Vertice makePathTo(double[] odoPos){
		double[] iPos = new double[3];
		FactoryFloorRobot.getInstance().getOdometer().getPosition(iPos);
		//FactoryFloorRobot.getInstance().stop();
		map.reBuild(high.getSensorData());
		int[] mapTarg = Map.mapCoords(odoPos);
		int[] mapStart = Map.mapCoords(iPos);
		Vertice target = map.vertices[mapTarg[0]][mapTarg[1]];
		Vertice start = map.vertices[mapStart[0]][mapStart[1]];
		start.pathPrev=start;
		target.pathNext=target;
		if(buildPath(start, target)==-1){
			start.pathNext=start;
		}
		return start;
	}
	/**builds the path between the start and end Vertice
	 * @param start - initial location
	 * @param target - target location
	 * @return 0 for sucess -1 for failure*/
	private int buildPath(Vertice start, Vertice target){
		if(target.score==-1)return -1;
		Vertice current=target;
		while(current!=start){
			current.pathPrev=prevVertice(current);
			current.pathPrev.pathNext=current;
			current=current.pathPrev;
		}
		optimizePath(start);
		return 0;
	}
	/**helper method for build Path*/
	private Vertice prevVertice(Vertice v){
		Vertice prev = v.north;
		if(v.south.score<prev.score)prev=v.south;
		if(v.east.score<prev.score)prev=v.east;
		if(v.west.score<prev.score)prev= v.west;
		return prev;
	}
	
	/**optimizes the path by removing the nodes that are all in a straight line from the path
	 * @param start -  the starting Vertice with a pathNext!=null*/
	private void optimizePath(Vertice start){
		int axis;
		Vertice initial=start;
		Vertice temp=initial.pathNext;
		while(initial!=initial.pathNext){//while not at the end
			if(initial.coords[0]==temp.coords[0])
				axis=0;//X
			else
				axis=1;//Y

			while(temp.coords[axis]==temp.pathNext.coords[axis] && temp!=temp.pathNext){
				//while the path is along the same axis and not at the end of the path
				temp=temp.pathNext;//
			}
			//either the path has turned or we are at the end
			
			//remove intermediate nodes to straight path
			initial.pathNext=temp;
			temp.pathPrev=initial;
			
			initial=temp;
			temp=initial.pathNext;
		}
		
	}

}
